/*
 * Copyright (C) 2012 Realtek Semiconductor Corp.
 * All Rights Reserved.
 *
 * This program is the proprietary software of Realtek Semiconductor
 * Corporation and/or its licensors, and only be used, duplicated,
 * modified or distributed under the authorized license from Realtek.
 *
 * ANY USE OF THE SOFTWARE OTHER THAN AS AUTHORIZED UNDER
 * THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
 *
 * $Revision: 39101 $
 * $Date: 2013-06-24 04:35:27 -0500 (Fri, 03 May 2013) $
 *
 * Purpose : OMCI driver layer module defination
 *
 * Feature : The file includes the following modules and sub-modules
 *           (1) OMCI (G.984.4)
 *
 */




/****************************************************************/
/* Include Files                                                */
/****************************************************************/
#include <linux/list.h>
#include <linux/slab.h>

#include "omci_dm_cb.h"
#include "omci_drv.h"

#include <rtk/rt/rt_gpon.h>

/****************************************************************/
/* Symbol Definition                                            */
/****************************************************************/
#define CONFIG_BY_OMCI (0x1)
#define CONFIG_BY_DMM (0x2)



/****************************************************************/
/* Type Definition                                              */
/****************************************************************/
typedef struct {
	unsigned int                drvFlowId;
	struct list_head	        list;
} omci_us_flow_list_t;

typedef struct {
	unsigned int                tconQUsedMask;
	struct list_head	        flowList;
} omci_us_flow_info_t;


typedef struct {
    unsigned int        configMask;/*0x1=>OMCI,0x2=>DMM*/
    unsigned int        priority;/*0~7,0=>MIN,7=>MAX*/
    unsigned int        pir;
    unsigned int        cir;
    unsigned int        scheduleType;
    unsigned int        Weight;   
    unsigned int        usedCnt;
    struct list_head    expandFlowHead;
}omci_us_flow_cfg_t;

typedef struct {
    unsigned int        logicFlowId;
    unsigned int        drvFlowId;    /*driver gem flow index */    
    unsigned int        pir;        
    unsigned int        cir;          
    rt_gpon_usFlow_t    drvFlowCfg;
	struct list_head		list;
}omci_us_flow_entry_t;

/****************************************************************/
/* Data Declaration                                             */
/****************************************************************/
//static omci_flow_map_t *pUsFlowMap;
static omci_us_flow_cfg_t *pUsDrvFlowCfg;
static unsigned int gMaxFlow;
static struct list_head    gOmciUsFlowHead;

/****************************************************************/
/* Function Declaration                                         */
/****************************************************************/
omci_us_flow_entry_t*
__omci_us_flow_entry_find_by_logicFlowId(unsigned int logicFlowId)
{
    omci_us_flow_entry_t *curEntry = NULL;
    struct list_head *next = NULL, *tmp=NULL;

    list_for_each_safe(next,tmp,&gOmciUsFlowHead)
    {

        curEntry = list_entry(next,omci_us_flow_entry_t,list);

        if(logicFlowId == curEntry->logicFlowId)
        {
            return curEntry;
        }
    }
    return NULL;
}


omci_us_flow_entry_t*
__omci_us_flow_entry_find_by_drvFlowId(unsigned int drvFlowId)
{
    omci_us_flow_entry_t *curEntry = NULL;
    struct list_head *next = NULL, *tmp=NULL;

    list_for_each_safe(next,tmp,&gOmciUsFlowHead)
    {

        curEntry = list_entry(next,omci_us_flow_entry_t,list);

        if(drvFlowId == curEntry->drvFlowId)
        {
            return curEntry;
        }
    }
    return NULL;
}

int __omci_us_flow_entry_del(unsigned int logicFlowId)
{
    struct list_head        *pEntry;
    struct list_head        *pTmpEntry;
    omci_us_flow_entry_t    *pEntryData;

    list_for_each_safe(pEntry, pTmpEntry, &gOmciUsFlowHead)
    {
        pEntryData = list_entry(pEntry, omci_us_flow_entry_t, list);

        if (pEntryData->logicFlowId == logicFlowId)
        {
            list_del(&pEntryData->list);

            kfree(pEntryData);

            return OMCI_ERR_OK;
        }
    }

    return OMCI_ERR_OK;
}


int __omci_us_flow_entry_add(unsigned int usLogicFlowId, unsigned usDrvFlowId, rt_gpon_usFlow_t *pUsFlow)
{
    omci_us_flow_entry_t *p;

    p = (omci_us_flow_entry_t*)kzalloc(sizeof(omci_us_flow_entry_t),GFP_KERNEL);
    p->logicFlowId = usLogicFlowId;
    p->drvFlowId = usDrvFlowId;
    memcpy(&(p->drvFlowCfg), pUsFlow, sizeof(rt_gpon_usFlow_t));
    list_add_tail(&p->list,&gOmciUsFlowHead);
    return OMCI_ERR_OK;
}



static int __dumpUsFlowMap(uint32 maxFlow)
{
    unsigned int flowId;
    omci_us_flow_entry_t *pflowEntry = NULL;
    
    printk("\r\n");
    printk("[Dump US Flow Map]\r\n");
    printk("Logic-Flow  Drv-Flow  T-CONT    TcQueue   PIR         CIR\r\n");
    printk("---------------------------------------------------------\r\n");
   
    for(flowId=0;flowId<maxFlow;flowId++)
    {
        pflowEntry = __omci_us_flow_entry_find_by_logicFlowId(flowId);
        if(pflowEntry)
        {
            printk("%-10d  %-8d  %-8d   %-8d %-12u %-12u\r\n", 
                flowId, pflowEntry->drvFlowId, 
                pflowEntry->drvFlowCfg.tcontId,
                pflowEntry->drvFlowCfg.tcQueueId,
                pflowEntry->pir,
                pflowEntry->cir);
        }
    }

    return OMCI_ERR_OK;
}


static int __dumpUsDrvFlowCfg(uint32 maxFlow)
{
    unsigned int flowId;

    printk("\r\n");
    printk("[Dump US DRV flow]\r\n");
    printk("Flow ID   priority  pir          cir          schType  weight    usedCnt  configMask\r\n");
    printk("------------------------------------------------------------------------------------\r\n");
   
    for(flowId=0;flowId<maxFlow;flowId++)
    {

        if(pUsDrvFlowCfg[flowId].configMask != 0x0)
        {
            printk("%-8u  %-8u  %-12u %-12u %-8u %-8u  %-8u 0x%x\r\n", 
                flowId, 
                pUsDrvFlowCfg[flowId].priority, 
                pUsDrvFlowCfg[flowId].pir,
                pUsDrvFlowCfg[flowId].cir,
                pUsDrvFlowCfg[flowId].scheduleType,
                pUsDrvFlowCfg[flowId].Weight,
                pUsDrvFlowCfg[flowId].usedCnt,
                pUsDrvFlowCfg[flowId].configMask);
        }
    }

    return OMCI_ERR_OK;
}



static int __assignUsDrvFlowId(unsigned int usLogicFlowId, unsigned int *pUsDrvFlowId,  rt_gpon_usFlow_t *pUsFlow)
{
    omci_us_flow_entry_t *pflowEntry = NULL;
    unsigned int i;
    int ret;
    if(!pUsDrvFlowId)
    {
        return OMCI_ERR_FAILED;
    }


    if(usLogicFlowId >= gMaxFlow)
    {
        return OMCI_ERR_FAILED;
    }

    pflowEntry = __omci_us_flow_entry_find_by_logicFlowId(usLogicFlowId);

    for(i=0;i<gMaxFlow;i++)
    {
        if(0x0 == pUsDrvFlowCfg[i].configMask)
        {
            *pUsDrvFlowId = i;
            ret = __omci_us_flow_entry_add(usLogicFlowId, i, pUsFlow);
            
             pUsDrvFlowCfg[i].configMask |= CONFIG_BY_OMCI;
             
            OMCI_LOG(OMCI_LOG_LEVEL_INFO,"%s() %d usLogicFlowId:%d drvFlowId:%d\n",
                __FUNCTION__, __LINE__, usLogicFlowId, *pUsDrvFlowId );

            return OMCI_ERR_OK; 
        }

    }

    return OMCI_ERR_FAILED;
    
}

static int __getUsDrvFlowId(unsigned int usLogicFlowId, unsigned int *pUsDrvFlowId)
{
    omci_us_flow_entry_t *pflowEntry = NULL;
    if(usLogicFlowId >= gMaxFlow)
    {
        return OMCI_ERR_FAILED;
    }

    pflowEntry = __omci_us_flow_entry_find_by_logicFlowId(usLogicFlowId);


    if(pflowEntry)
    {
        *pUsDrvFlowId = pflowEntry->drvFlowId;
        OMCI_LOG(OMCI_LOG_LEVEL_INFO,"%s %d usLogicFlowId:%d drvFlowId:%d\n",
                __FUNCTION__, __LINE__, usLogicFlowId, *pUsDrvFlowId );
        return OMCI_ERR_OK;
    }

    return OMCI_ERR_FAILED;    
}

static int __delUsDrvFlowId(unsigned int usLogicFlowId)
{
    omci_us_flow_entry_t *pflowEntry = NULL;
    unsigned int drvFlowId;
    
    if(usLogicFlowId >= gMaxFlow)
    {
        return OMCI_ERR_FAILED;
    }

    pflowEntry = __omci_us_flow_entry_find_by_logicFlowId(usLogicFlowId);
    if(NULL == pflowEntry)
    {
        return OMCI_ERR_OK;
    }

    drvFlowId = pflowEntry->drvFlowId;

    __omci_us_flow_entry_del(usLogicFlowId);
    
    pUsDrvFlowCfg[drvFlowId].configMask &= ~CONFIG_BY_OMCI;
    OMCI_LOG(OMCI_LOG_LEVEL_INFO,"%s %d usLogicFlowId:%d",
            __FUNCTION__, __LINE__, usLogicFlowId);
    return OMCI_ERR_OK;   
}


static int __delAllUsDrvFlowId(void)
{
    struct list_head        *pEntry =NULL;
    struct list_head        *pTmpEntry = NULL;
    omci_us_flow_entry_t    *pEntryData = NULL;

    
    list_for_each_safe(pEntry, pTmpEntry, &gOmciUsFlowHead)
    {
        pEntryData = list_entry(pEntry, omci_us_flow_entry_t, list);
        pUsDrvFlowCfg[pEntryData->drvFlowId].configMask &= ~CONFIG_BY_OMCI;

        list_del(&pEntryData->list);

        kfree(pEntryData);

    }

    return OMCI_ERR_OK;   
}



static int __requestEmptyDrvFlow(unsigned int requestNum, unsigned int *pUsDrvFlowId)
{
    unsigned int i;
    unsigned int emptyNum = 0;

    /*Check empty number*/
    for(i=0;i<gMaxFlow;i++)
    {
        if(0x0 == pUsDrvFlowCfg[i].configMask)
        { 
            emptyNum++;
            if(requestNum == emptyNum)
                break;
        }

    }
    
    if(requestNum != emptyNum)
    {
        return OMCI_ERR_FAILED;        
    }
    
    /*assign flow number*/
    requestNum = 0; 
    for(i=0;i<gMaxFlow;i++)
    {
        if(0x0 == pUsDrvFlowCfg[i].configMask)
        {
            *pUsDrvFlowId = i; 
            requestNum++;
            if(requestNum == emptyNum)
                return OMCI_ERR_OK; 

            pUsDrvFlowId++;
        }

    }
    return OMCI_ERR_FAILED;
}



static int __add_expand_flow_to_db(unsigned int oriFlowId, omci_dm_flow_config_t *pFlowCfg)
{
    omci_us_flow_list_t     *pEntryData;

    /*expand flow add orignal flow list*/
    if(pFlowCfg->flowId != oriFlowId)
    {
        pEntryData = (omci_us_flow_list_t*)kmalloc(sizeof(omci_us_flow_list_t),GFP_KERNEL);
        pEntryData->drvFlowId = pFlowCfg->flowId;

        list_add_tail(&pEntryData->list, &pUsDrvFlowCfg[oriFlowId].expandFlowHead);
    }

    /*Save per flow database*/
    pUsDrvFlowCfg[pFlowCfg->flowId].priority        = pFlowCfg->priority; 
    pUsDrvFlowCfg[pFlowCfg->flowId].pir             = pFlowCfg->pir; 
    pUsDrvFlowCfg[pFlowCfg->flowId].cir             = pFlowCfg->cir; 
    pUsDrvFlowCfg[pFlowCfg->flowId].scheduleType    = pFlowCfg->scheduleType; 
    pUsDrvFlowCfg[pFlowCfg->flowId].Weight          = pFlowCfg->Weight; 
    
    return OMCI_ERR_OK;
}

static int __qos_delete_expand_flow(unsigned int oriFlowId)
{
    int ret;
    struct list_head        *pEntry = NULL;
    struct list_head        *pTmpEntry = NULL;
    omci_us_flow_list_t     *pEntryData = NULL;
    omci_us_flow_entry_t    *pflowEntry = NULL;

    /*Delete all expand flow*/
    list_for_each_safe(pEntry, pTmpEntry, &pUsDrvFlowCfg[oriFlowId].expandFlowHead)
    {
        pEntryData = list_entry(pEntry, omci_us_flow_list_t, list);

        ret = rt_gpon_usFlow_del(pEntryData->drvFlowId);
        if(RT_ERR_OK != ret)
        {
            OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d set us usDrvFlowId:%u fail,ret:%d",
                __FUNCTION__,__LINE__, pEntryData->drvFlowId, ret);           
        }
        pUsDrvFlowCfg[pEntryData->drvFlowId].configMask &= ~CONFIG_BY_DMM;

        list_del(&pEntryData->list);

        kfree(pEntryData);
        
    }
 
    /*Rollback to orignal OMCI config*/
    pflowEntry = __omci_us_flow_entry_find_by_drvFlowId(oriFlowId);
            OMCI_LOG(OMCI_LOG_LEVEL_INFO,"%s() %d ",
                __FUNCTION__,__LINE__);
    if(pflowEntry)
    {
        ret = rt_gpon_usFlow_set(oriFlowId, &pflowEntry->drvFlowCfg);
        if(RT_ERR_OK != ret)
        {
            OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d set us usDrvFlowId:%u fail,ret:%d",
                __FUNCTION__, __LINE__, oriFlowId, ret);
            
            return OMCI_ERR_FAILED;            
        }
        pUsDrvFlowCfg[oriFlowId].priority = pflowEntry->drvFlowCfg.tcQueueId;
    }


    pUsDrvFlowCfg[oriFlowId].configMask &= ~CONFIG_BY_DMM;
    
    INIT_LIST_HEAD(&pUsDrvFlowCfg[oriFlowId].expandFlowHead);

    return OMCI_ERR_OK;
}


static int __qos_add_expand_flow(unsigned int flowId, unsigned int flowNum, omci_dm_flow_config_t *pExpflowCfg)
{

    unsigned int expandFlowNum;
    unsigned int *pUsDrvFlowId =NULL;
    int ret,i;
    rt_gpon_usFlow_t usFlowCfg;
    rt_gpon_queueCfg_t queuecfg;

   /*Add expand flow*/
    expandFlowNum = flowNum-1;
    pUsDrvFlowId = kmalloc(sizeof(unsigned int) * expandFlowNum, GFP_KERNEL);
    if(!pUsDrvFlowId)
    {
        return OMCI_ERR_FAILED;
    }

    if(OMCI_ERR_FAILED == __requestEmptyDrvFlow(expandFlowNum, pUsDrvFlowId))
    {
        OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d : request flow fail,num:%d",
            __FUNCTION__, __LINE__, flowNum-1);
        kfree(pUsDrvFlowId);
        return OMCI_ERR_FAILED;       
    }

    pExpflowCfg[0].flowId = flowId;
    for(i=0;i<expandFlowNum;i++)
    {
        pExpflowCfg[i+1].flowId = pUsDrvFlowId[i];
    }

    kfree(pUsDrvFlowId);


    ret = rt_gpon_usFlow_get(flowId,&usFlowCfg);
    if(RT_ERR_OK != ret)
    {
        OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d get us US FlowId:%u fail,ret:%d ",
            __FUNCTION__, __LINE__, flowId, ret);
        return OMCI_ERR_FAILED;            
    }

    /*Write to driver*/
    /*gemPortId/T-CONT/ase is same as orignal FLOW ID*/
    for(i=0;i<flowNum;i++)
    {
        queuecfg.weight         = pExpflowCfg[i].Weight;
        queuecfg.scheduleType   = pExpflowCfg[i].scheduleType;
        queuecfg.pir            = pExpflowCfg[i].pir;
        queuecfg.cir            = pExpflowCfg[i].cir;

        if(RT_ERR_OK != rt_gpon_ponQueue_set(usFlowCfg.tcontId, pExpflowCfg[i].priority,&queuecfg))
        {
            OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d rt set upstream queue fail\n",__FUNCTION__,__LINE__);
            return OMCI_ERR_FAILED;
        }

        usFlowCfg.tcQueueId  = pExpflowCfg[i].priority;

        ret = rt_gpon_usFlow_set(pExpflowCfg[i].flowId,&usFlowCfg);
        if(RT_ERR_OK != ret)
        {
            OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d set us usDrvFlowId:%u fail,ret:%d",
                __FUNCTION__,__LINE__, flowId, ret);
            return OMCI_ERR_FAILED;            
        }

        /*Write do database*/
        if(OMCI_ERR_OK != __add_expand_flow_to_db(flowId, &pExpflowCfg[i]))
        {
            OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d set us usDrvFlowId:%u fail",__FUNCTION__,__LINE__, flowId);
            return OMCI_ERR_FAILED;  
        }
        pUsDrvFlowCfg[pExpflowCfg[i].flowId].configMask |= CONFIG_BY_DMM;

    }

   
    return OMCI_ERR_OK; 
}


/*If multiple us flow point to one GEM, the function will force different GEM to different T-CONT queue*/
int  omci_qos_force_usFlow_to_diff_queue(unsigned int tcontId,unsigned int tcQueueId)
{
    int ret;
    rt_gpon_usFlow_t usFlowCfg;
    unsigned int matchCnt = 0;
    omci_us_flow_entry_t *curEntry = NULL;
    struct list_head *next = NULL, *tmp=NULL;
    unsigned int tcQid = 0;
    rt_gpon_queueCfg_t queueCfg;


    OMCI_LOG(OMCI_LOG_LEVEL_DBG,"%s() %d ",__FUNCTION__,__LINE__);

    list_for_each_safe(next,tmp,&gOmciUsFlowHead)
    {
        curEntry = list_entry(next,omci_us_flow_entry_t,list);

        ret = rt_gpon_usFlow_get(curEntry->drvFlowId, &usFlowCfg);
        if(RT_ERR_OK != ret)
        {
            OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d get us US FlowId:%u fail,ret:%d ",
                __FUNCTION__, __LINE__, curEntry->drvFlowId, ret);
            return OMCI_ERR_FAILED;            
        }

        if(curEntry->drvFlowCfg.tcontId == tcontId &&
           curEntry->drvFlowCfg.tcQueueId  == tcQueueId)
        {
            matchCnt++;
        }

    }
    OMCI_LOG(OMCI_LOG_LEVEL_DBG,"%s() %d matchCnt:%d",__FUNCTION__, __LINE__ ,matchCnt);

    if (matchCnt <= 1)
    {
        return OMCI_ERR_OK;
    }

    if(RT_ERR_OK != rt_gpon_ponQueue_get(tcontId, tcQueueId,&queueCfg))
    {
        OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d rt get upstream queue fail\n",__FUNCTION__,__LINE__);
        return OMCI_ERR_FAILED;
    }


    curEntry = NULL;
    list_for_each_safe(next,tmp,&gOmciUsFlowHead)
    {
        curEntry = list_entry(next,omci_us_flow_entry_t,list);

        ret = rt_gpon_usFlow_get(curEntry->drvFlowId, &usFlowCfg);
        if(RT_ERR_OK != ret)
        {
            OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d get us US FlowId:%u fail,ret:%d ",
                __FUNCTION__, __LINE__, curEntry->drvFlowId, ret);
            return OMCI_ERR_FAILED;            
        }

        if(curEntry->drvFlowCfg.tcontId == tcontId &&
           curEntry->drvFlowCfg.tcQueueId  == tcQueueId)
        {
            queueCfg.pir    = curEntry->pir;
            queueCfg.cir    = curEntry->cir;
            
            OMCI_LOG(OMCI_LOG_LEVEL_INFO,"%s() %d flow:%d tcQid:%d pir:%d cir:%d",
                __FUNCTION__, __LINE__ ,
                curEntry->drvFlowId,
                tcQid,queueCfg.pir,
                queueCfg.cir);
            
            if(RT_ERR_OK != rt_gpon_ponQueue_set(usFlowCfg.tcontId, tcQid,&queueCfg))
            {
                OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d rt set upstream queue fail\n",__FUNCTION__,__LINE__);
                return OMCI_ERR_FAILED;
            }

            usFlowCfg.tcQueueId = tcQid;
            ret = rt_gpon_usFlow_set(curEntry->drvFlowId, &usFlowCfg);
            if(RT_ERR_OK != ret)
            {
                OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d set us US FlowId:%u fail,ret:%d ",
                    __FUNCTION__, __LINE__, curEntry->drvFlowId, ret);
                return OMCI_ERR_FAILED;            
            }

            tcQid++;
        }

    }
    return OMCI_ERR_OK; 

}


int
omci_qos_flow_init(unsigned int maxFlow)
{
    unsigned int flowId;

    pUsDrvFlowCfg = kzalloc(sizeof(omci_us_flow_cfg_t) * maxFlow, GFP_KERNEL);
    if (!pUsDrvFlowCfg)
    {
        OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d : init drvFlowCfg fail",__FUNCTION__,__LINE__);
        return OMCI_ERR_FAILED;
    }

    for(flowId=0;flowId<maxFlow;flowId++)
    {
        INIT_LIST_HEAD(&pUsDrvFlowCfg[flowId].expandFlowHead);
    }

    INIT_LIST_HEAD(&gOmciUsFlowHead);
    gMaxFlow = maxFlow;
  
    return OMCI_ERR_OK;
}



int omci_qos_GetUsDrvFlowId(unsigned int usLogicFlowId, unsigned int *pUsDrvFlowId)
{
    int ret;
    if(!pUsDrvFlowId)
    {
        return OMCI_ERR_FAILED;
    }

    if(usLogicFlowId >= gMaxFlow)
    {
        return OMCI_ERR_FAILED;
    }

    ret = __getUsDrvFlowId(usLogicFlowId, pUsDrvFlowId);
    if(ret != OMCI_ERR_OK)
    {
        OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d get flow:%d fail",__FUNCTION__,__LINE__,usLogicFlowId);
        return OMCI_ERR_FAILED; 
    }

    return OMCI_ERR_OK;    
}

int
omci_qos_flow_dump(unsigned int maxFlow)
{
    __dumpUsFlowMap(maxFlow);
    __dumpUsDrvFlowCfg(maxFlow);

    return OMCI_ERR_OK;
}


int
omci_qos_usFlow_set(unsigned int usFlowId, rt_gpon_usFlow_t *pUsFlow)
{
    int ret ;
    unsigned int usDrvFlowId;
    struct list_head        *pEntry = NULL;
    struct list_head        *pTmpEntry = NULL;
    omci_us_flow_list_t     *pEntryData = NULL;
    rt_gpon_usFlow_t usFlowCfg;
    omci_us_flow_entry_t *pflowEntry = NULL;
    BOOL isAesStateChange = FALSE;

    pflowEntry = __omci_us_flow_entry_find_by_logicFlowId(usFlowId);

    if(pflowEntry)
    {
        usDrvFlowId = pflowEntry->drvFlowId;

        if(pUsFlow->aesEn != pflowEntry->drvFlowCfg.aesEn)
        {
            isAesStateChange = TRUE;
            OMCI_LOG(OMCI_LOG_LEVEL_INFO,"%s %d AES state %d to %d\n",
                __FUNCTION__, __LINE__, pUsFlow->aesEn, pflowEntry->drvFlowCfg.aesEn);

        }
        memcpy(&(pflowEntry->drvFlowCfg), pUsFlow, sizeof(rt_gpon_usFlow_t));
    }
    else
    {
        ret = __assignUsDrvFlowId(usFlowId, &usDrvFlowId, pUsFlow);
        if(ret != OMCI_ERR_OK)
        {
            OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d assign drv gemflow:%d fail",__FUNCTION__,__LINE__,usFlowId);
            return OMCI_ERR_FAILED; 
        }
    }
  
    ret = rt_gpon_usFlow_set(usDrvFlowId, pUsFlow);
    if(RT_ERR_OK != ret)
    {
        OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d set us usDrvFlowId:%u fail,ret:%d",
            __FUNCTION__, __LINE__, usDrvFlowId, ret);
        
        return OMCI_ERR_FAILED;            
    }

    /*Save per flow database*/
    pUsDrvFlowCfg[usDrvFlowId].priority = pUsFlow->tcQueueId; 


	OMCI_LOG(OMCI_LOG_LEVEL_WARN,"%s() %d usDrvFlowId:%u",
		__FUNCTION__, __LINE__, usDrvFlowId);


    /*Update all expand flow if aes state change*/
    if(TRUE == isAesStateChange)
    {
        list_for_each_safe(pEntry, pTmpEntry, &pUsDrvFlowCfg[usDrvFlowId].expandFlowHead)
        {
            pEntryData = list_entry(pEntry, omci_us_flow_list_t, list);

            ret = rt_gpon_usFlow_get(pEntryData->drvFlowId,&usFlowCfg);
            if(RT_ERR_OK != ret)
            {
                OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d get us US FlowId:%u fail,ret:%d ",
                __FUNCTION__, __LINE__, usDrvFlowId, ret);
                return OMCI_ERR_FAILED; 		   
            }

            usFlowCfg.aesEn = pUsFlow->aesEn;

            ret = rt_gpon_usFlow_set(pEntryData->drvFlowId, &usFlowCfg);
            if(RT_ERR_OK != ret)
            {
                OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d set us usDrvFlowId:%u fail,ret:%d",
                __FUNCTION__, __LINE__, usDrvFlowId, ret);

                return OMCI_ERR_FAILED; 		   
            }
        }
    }


    return OMCI_ERR_OK;
}


int
omci_qos_usFlow_del(unsigned int usFlowId)
{
    int ret ;
    unsigned int usDrvFlowId;
    
    ret = __getUsDrvFlowId(usFlowId, &usDrvFlowId);
    if(ret != OMCI_ERR_OK)
    {
        OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d get flow:%d fail",__FUNCTION__,__LINE__,usFlowId);
        return OMCI_ERR_FAILED; 
    }

    ret = rt_gpon_usFlow_del(usDrvFlowId);
    if(RT_ERR_OK != ret)
    {
        OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d del us usDrvFlowId:%u fail,ret:%d",
            __FUNCTION__, __LINE__, usDrvFlowId, ret);

        return OMCI_ERR_FAILED;            
    }

    ret = __delUsDrvFlowId(usFlowId);
    if(ret != OMCI_ERR_OK)
    {
        OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d del flow:%d fail",__FUNCTION__,__LINE__,usFlowId);
        return OMCI_ERR_FAILED; 
    }

    return OMCI_ERR_OK;
}

int
omci_qos_usFlow_delAll(void)
{
    int ret ;
    
    ret = rt_gpon_usFlow_delAll();
    if(RT_ERR_OK != ret)
    {
        OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d del all us flow fail,ret:%d",
            __FUNCTION__, __LINE__, ret);

        return OMCI_ERR_FAILED;            
    }

    ret = __delAllUsDrvFlowId();
    if(ret != OMCI_ERR_OK)
    {
        OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d del flow fail",__FUNCTION__,__LINE__);
        return OMCI_ERR_FAILED; 
    }

    return OMCI_ERR_OK;
}


int
omci_qos_usFlowRate_set(unsigned int usFlowId, unsigned int pir, unsigned int cir)
{
    omci_us_flow_entry_t *pflowEntry = NULL;
    int ret;
    rt_gpon_usFlow_t usFlowCfg;

    pflowEntry = __omci_us_flow_entry_find_by_logicFlowId(usFlowId);

    if(pflowEntry)
    {
        /*Update PIR/CIR to database*/
        pflowEntry->pir = pir;
        pflowEntry->cir = cir;

        ret = rt_gpon_usFlow_get(pflowEntry->drvFlowId, &usFlowCfg);
        if(RT_ERR_OK != ret)
        {
            OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d get us US FlowId:%u fail,ret:%d ",
                __FUNCTION__, __LINE__, pflowEntry->drvFlowId, ret);
            return OMCI_ERR_FAILED;            
        }
        OMCI_LOG(OMCI_LOG_LEVEL_INFO,"%s() %d flow:%d pir:%d cir:%d ",
            __FUNCTION__,
            __LINE__,
            pflowEntry->drvFlowId,
            pflowEntry->pir,
            pflowEntry->cir);

        if (OMCIDRV_FEATURE_ERR_OK == omcidrv_feature_api(FEATURE_KAPI_BDP_00000800))
        {
            omci_qos_force_usFlow_to_diff_queue(pflowEntry->drvFlowCfg.tcontId, pflowEntry->drvFlowCfg.tcQueueId);
        }
        
    }
    return OMCI_ERR_OK;

}







int 
omci_dmm_qos_flow_set(const omci_dm_flow_config_t *pFlowCfg)
{
    int ret;
    rt_gpon_usFlow_t usFlowCfg;
    rt_gpon_queueCfg_t queuecfg;

    if (!pFlowCfg)
        return OMCI_ERR_FAILED;
    
    if(0x0 == pUsDrvFlowCfg[pFlowCfg->flowId].configMask)
    {
        OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d set us usDrvFlowId:%u is not exist",
            __FUNCTION__,__LINE__, pFlowCfg->flowId);
        return OMCI_ERR_FAILED; 
    }

    ret = rt_gpon_usFlow_get(pFlowCfg->flowId,&usFlowCfg);
    if(RT_ERR_OK != ret)
    {
        OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s() %d get us US FlowId:%u fail,ret:%d ",
            __FUNCTION__, __LINE__, pFlowCfg->flowId, ret);
        return OMCI_ERR_FAILED;            
    }

    memset(&queuecfg, 0, sizeof(queuecfg));
    queuecfg.weight         = pFlowCfg->Weight;
    queuecfg.scheduleType   = pFlowCfg->scheduleType;
    queuecfg.pir            = pFlowCfg->pir;
    queuecfg.cir            = pFlowCfg->cir;

    if(RT_ERR_OK != rt_gpon_ponQueue_set(usFlowCfg.tcontId, pFlowCfg->priority, &queuecfg))
    {
        OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s %d rt set upstream queue fail\n",__FUNCTION__,__LINE__);
        return OMCI_ERR_FAILED;
    }


    /*priority change, update US gem flow*/
    if(pFlowCfg->priority != pUsDrvFlowCfg[pFlowCfg->flowId].priority)
    {
        ret = rt_gpon_usFlow_set(pFlowCfg->flowId,&usFlowCfg);
        if(RT_ERR_OK != ret)
        {
            OMCI_LOG(OMCI_LOG_LEVEL_ERR,"%s %d set us usDrvFlowId:%u fail,ret:%d",
                __FUNCTION__,__LINE__, pFlowCfg->flowId, ret);
            return OMCI_ERR_FAILED;            
        }    

    }

    /*Save per flow database*/
    pUsDrvFlowCfg[pFlowCfg->flowId].priority        = pFlowCfg->priority; 
    pUsDrvFlowCfg[pFlowCfg->flowId].pir             = pFlowCfg->pir; 
    pUsDrvFlowCfg[pFlowCfg->flowId].cir             = pFlowCfg->pir; 
    pUsDrvFlowCfg[pFlowCfg->flowId].scheduleType    = pFlowCfg->scheduleType; 
    pUsDrvFlowCfg[pFlowCfg->flowId].Weight          = pFlowCfg->Weight;

    return OMCI_ERR_OK;

}

int
omci_dmm_qos_flow_get(omci_dm_flow_config_t *pFlowCfg)
{
    unsigned int usDrvFlowId;  

    if(!pFlowCfg)
        return OMCI_ERR_FAILED;

    if(pFlowCfg->flowId >= gMaxFlow)
        return OMCI_ERR_FAILED;

    usDrvFlowId = pFlowCfg->flowId;

    if(0x0 == pUsDrvFlowCfg[usDrvFlowId].configMask)
        return OMCI_ERR_ENTRY_NOT_EXIST;

    pFlowCfg->priority      = pUsDrvFlowCfg[usDrvFlowId].priority; 
    pFlowCfg->pir           = pUsDrvFlowCfg[usDrvFlowId].pir; 
    pFlowCfg->cir           = pUsDrvFlowCfg[usDrvFlowId].cir; 
    pFlowCfg->scheduleType  = pUsDrvFlowCfg[usDrvFlowId].scheduleType; 
    pFlowCfg->Weight        = pUsDrvFlowCfg[usDrvFlowId].Weight; 
    
    return OMCI_ERR_OK;   
}

int 
omci_dmm_qos_flow_expand(unsigned int flowId, unsigned int flowNum, omci_dm_flow_config_t *pExpflowCfg)
{
    int ret = OMCI_ERR_OK;
    struct list_head        *pEntry = NULL;
    struct list_head        *pTmpEntry = NULL;
    omci_us_flow_list_t     *pEntryData = NULL;
    int i;

    /* parameter check */
    if(!pExpflowCfg)
        return OMCI_ERR_FAILED;

    if(flowId >= gMaxFlow)
        return OMCI_ERR_FAILED;
    
    if(flowNum<1 || flowNum >= gMaxFlow)
        return OMCI_ERR_FAILED;
    
    /*Delete expand flow*/
    if(flowNum == 1)
    {
        if(1 == pUsDrvFlowCfg[flowId].usedCnt)
        {
            ret =__qos_delete_expand_flow(flowId);
            if(OMCI_ERR_OK == ret)
            {
                pUsDrvFlowCfg[flowId].usedCnt = 0;
            }
            OMCI_LOG(OMCI_LOG_LEVEL_DBG,"%s() %d ,delete expand flow:%d", __FUNCTION__, __LINE__, flowId);
        }
        else if(pUsDrvFlowCfg[flowId].usedCnt > 1)
        {
            pUsDrvFlowCfg[flowId].usedCnt--;
            OMCI_LOG(OMCI_LOG_LEVEL_DBG,"%s() %d ", __FUNCTION__, __LINE__);
        }
		

    }
    else
    {
        if(0 == pUsDrvFlowCfg[flowId].usedCnt)
        {
            ret =__qos_add_expand_flow(flowId, flowNum, pExpflowCfg);

            OMCI_LOG(OMCI_LOG_LEVEL_DBG,"%s() %d ,expand flow:%d", __FUNCTION__, __LINE__, flowId);

            if(OMCI_ERR_OK == ret)
            {
                pUsDrvFlowCfg[flowId].usedCnt = 1;
            }
        }
        else
        {
            pUsDrvFlowCfg[flowId].usedCnt++;
            /*The flow is already expanded, just need to fill the flow ID*/
            for(i=0;i<flowNum;i++)
            {
                if(pExpflowCfg[i].priority == 0)
                {
                    pExpflowCfg[i].flowId = flowId;
                }
                else
                {
                    list_for_each_safe(pEntry, pTmpEntry, &pUsDrvFlowCfg[flowId].expandFlowHead)
                    {
                        pEntryData = list_entry(pEntry, omci_us_flow_list_t, list);
                        if(pUsDrvFlowCfg[pEntryData->drvFlowId].priority == pExpflowCfg[i].priority)
                        {
                            pExpflowCfg[i].flowId = pEntryData->drvFlowId;
                        }
                    }
                }
            }

            OMCI_LOG(OMCI_LOG_LEVEL_DBG,"%s() %d ", __FUNCTION__, __LINE__);
        }
    }

    return ret;
 
}




/*EXPORT function*/
EXPORT_SYMBOL(omci_dmm_qos_flow_set);
EXPORT_SYMBOL(omci_dmm_qos_flow_get);
EXPORT_SYMBOL(omci_dmm_qos_flow_expand);
